#include "PID.h"

PID Fire_LM_PID(13, 0.05, 0, 1000, 16000);
PID Fire_RM_PID(13, 0.05, 0, 1000, 16000);
#if (YUNTAIMODE == 0)

PID Pitch_W_PID(400, 0, 0, 5000, 25000);
PID Pitch_Angle_PID(1, 0, 0, 100, 5000);

//PID Pitch_W_PID(130, 0, 0, 5000, 25000);
//PID Pitch_Angle_PID(0.8, 0.07, 0.4, 100, 5000);
// PID Pitch_W_PID(30, 4.4, 5, 1000, 16000);
// PID Pitch_Angle_PID(7, 0, 0.3, 500, 5000);
#endif

#if (YUNTAIMODE == 1)
// PID Pitch_Vel_PID(100, 10, 0, 1000, 16000);
PID Pitch_Pos_PID(100, 0, 0, 1000, 16000);
#endif

PID::PID(float p, float i, float d, float maxi, float maxout) : kp(p), ki(i), kd(d), maxIntegral(maxi), maxOutput(maxout)
{
    error = 0;
    lastError = 0;
    integral = 0;
    output = 0;
    reference = 0;
    feedback = 0;
    dout = 0;
    pout = 0;
    iout = 0;
    Dir = 1;
    output_rate = 0;
    output_prev = 0;
}

// 位置式PID
void PID::PosPIDCalc(float output_ramp)
{

    // 更新数据
    lastError = error;            // 将旧error存起来
    error = reference - feedback; // 计算新error
                                  // 计算积分
    integral += error;
    // 计算微分
    dout = (error - lastError) * kd;
    // 计算比例
    pout = error * kp;
    // 积分限幅
    if (integral > maxIntegral)
        integral = maxIntegral;
    else if (integral < -maxIntegral)
        integral = -maxIntegral;
    iout = integral * ki;
    // 计算输出
    output = pout + dout + iout;
    if (output_ramp > 0)
    {
        // 对PID的变化速率进行限制
        float output_rate = (output - output_prev);
        if (output_rate > output_ramp)
            output = output_prev + output_ramp;
        else if (output_rate < -output_ramp)
            output = output_prev - output_ramp;
    }
    // 输出限幅
    if (output > maxOutput)
        output = maxOutput;
    else if (output < -maxOutput)
        output = -maxOutput;
}

// 增量式PID
void PID::DelPIDCalc()
{

    // 更新数据
    error = 0;
    error = reference - feedback; // 计算新error
    pout = (error - lastError) * kp;
    iout = error * ki;
    dout = (error - 2 * lastError + prev_error) * kd;

    output += pout + iout + dout;

    prev_error = lastError; // 将上上次的error作为预测值
    lastError = error;      // 将旧error存起来

    // 输出限幅
    if (output > maxOutput)
        output = maxOutput;
    else if (output < -maxOutput)
        output = -maxOutput;
}
